#include "skeleton.h"

Skeleton::Skeleton() {
}

Skeleton::Skeleton(const Skeleton& s) {
   rootPosition = s.rootPosition;
   trOrder = s.trOrder;
   bones = s.bones;
   hierarchy = s.hierarchy;
   father = s.father;
   rotation = s.rotation;
   
   translation0 = s.translation0;
   translation1 = s.translation1;
   scale0 = s.scale0;
   scale1 = s.scale1;
}

Skeleton::Skeleton(Point3D rp, Hierarchy h) {
   rootPosition = rp;
   hierarchy = h;
}

Skeleton::~Skeleton() {
}

void Skeleton::insertSon(string bone, string son) {
   hierarchy[bone].push_back(son);
   father[son] = bone;
}

void Skeleton::insertBone(Bone* b) {
   bones[b->getName()] = *b;
   rotation[b->getName()] = Quat();
   translation0[b->getName()] = Vector3D();
   translation1[b->getName()] = Vector3D();
   scale0[b->getName()] = 0.0;
   scale1[b->getName()] = 0.0;
}

/// rotates a vector by the given quaternion
Vector3D rotate(const Vector3D &v, const Quat &Q) {
   
   Quat Qv(v[0],v[1],v[2]);
   Quat qv = Q*Qv*conjugate(Q);
   return Vector3(qv[1],qv[2],qv[3]);
}

void Skeleton::rotateBone(string bone, Vector3D angles, string order,
      bool isRoot, bool isDeg) {
   
   string n = bone;
   queue<string> Q;
   Q.push(n);
   Sons sons;
   
   Vector3D axisX(1.0, 0.0, 0.0);
   Vector3D axisY(0.0, 1.0, 0.0);
   Vector3D axisZ(0.0, 0.0, 1.0);
   
   if (!isRoot) {
      //~ axisX = rotate(bones[n].getAxisX(), rotation[n]);
      //~ axisY = rotate(bones[n].getAxisY(), rotation[n]);
      //~ axisZ = rotate(bones[n].getAxisZ(), rotation[n]);
      axisX = bones[n].getAxisX();
      axisY = bones[n].getAxisY();
      axisZ = bones[n].getAxisZ();
   }
   
   while (!Q.empty()) {
      n = Q.front();
      Q.pop();
      Vector3D rot = bones[n].getDirection();
      
      for (int i = 0; i < (int) order.size(); i++) {
         if (order[i] == 'X' && angles[0] != 0) {
            
            rotation[n] = Quat::fromAxisAngle(axisX[0],axisX[1],axisX[2],angles[0]*(3.1415/180.0))*rotation[n];
            
            rot = rotate(angles[0], rot, axisX, isDeg);
            bones[n].setAxisX(rotate(angles[0], bones[n].getAxisX(), axisX, isDeg));
            bones[n].setAxisY(rotate(angles[0], bones[n].getAxisY(), axisX, isDeg));
            bones[n].setAxisZ(rotate(angles[0], bones[n].getAxisZ(), axisX, isDeg));
         } else if (order[i] == 'Y' && angles[1] != 0) {
            
            rotation[n] = Quat::fromAxisAngle(axisY[0],axisY[1],axisY[2],angles[1]*(3.1415/180.0))*rotation[n];
            
            rot = rotate(angles[1], rot, axisY, isDeg);
            bones[n].setAxisX(rotate(angles[1], bones[n].getAxisX(), axisY, isDeg));
            bones[n].setAxisY(rotate(angles[1], bones[n].getAxisY(), axisY, isDeg));
            bones[n].setAxisZ(rotate(angles[1], bones[n].getAxisZ(), axisY, isDeg));
         } else if (order[i] == 'Z' && angles[2] != 0) {
            
            rotation[n] = Quat::fromAxisAngle(axisZ[0],axisZ[1],axisZ[2],angles[2]*(3.1415/180.0))*rotation[n];
            
            rot = rotate(angles[2], rot, axisZ, isDeg);
            bones[n].setAxisX(rotate(angles[2], bones[n].getAxisX(), axisZ, isDeg));
            bones[n].setAxisY(rotate(angles[2], bones[n].getAxisY(), axisZ, isDeg));
            bones[n].setAxisZ(rotate(angles[2], bones[n].getAxisZ(), axisZ, isDeg));
         }
      }
      
      bones[n].setDirection(rot);
      
      // Refreshing queue
      sons = getSons(n);
      for (int i = 0; i < (int) sons.size(); i++) {
         Q.push(sons[i]);
      }
   }
}

Segment3D Skeleton::boneSegment(int i) {
   
   map<string, Bone>::iterator it = bones.begin();
   advance(it, i);
   
   string currentBone = it->second.getName();
   string currentFather = father[currentBone];
   
   return Segment3D(point(currentBone), point(currentFather));
}

Point3D Skeleton::point(string bone){
   if (bone == "root") {
      return rootPosition;
   } else {
      return point(father[bone]) + bones[bone].lenXdir();
   }
}

string Skeleton::boneName(int i) {
   
   map<string, Bone>::iterator it = bones.begin();
   advance(it, i);
   
   if (it == bones.end()) return "";
   return it->second.getName();
}

Quat Skeleton::getBoneQuaternion(int i) {
   
   string name = boneName(i);
   if (name.empty()) return Quat();
   return rotation[name];
}

Vector3 Skeleton::getBoneTranslation0(int i) {
   
   string name = boneName(i);
   if (name.empty()) return Vector3();
   return translation0[name];
}

Vector3 Skeleton::getBoneTranslation1(int i) {
   
   string name = boneName(i);
   if (name.empty()) return Vector3();
   return translation1[name];
}

Number Skeleton::getBoneScale0(int i) {
   
   string name = boneName(i);
   if (name.empty()) return 0.0;
   return scale0[name];
}

Number Skeleton::getBoneScale1(int i) {
   
   string name = boneName(i);
   if (name.empty()) return 0.0;
   return scale1[name];
}
